#ifndef _TRAJECTORYGENERATOR_H_
#define _TRAJECTORYGENERATOR_H_

#include "params.h"
#include "global.h"
#include <string>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

/*
    质心轨迹生成器
        角标命名规则：右上角开始顺时针命名
*/

class TrajectoryGenerator
{
private:
    double t_;          // 当前时间
    int n_;             // 当前为第n个周期

    int setflag_;       // 调用函数前先设置期望值

    double block_para_; // 堵转保护系数

    double v_x_d_B_;     // 期望x速度(B系)
    double v_y_d_B_;     // 期望y速度(B系)
    double omega_z_d_B_; // 期望绕z角速度(B系)

    Vector3d p_0_com_d_O_; // 上一控制周期求得此刻期望位置
    Vector3d v_0_com_d_O_; // 上一控制周期求得此刻期望速度
    Vector3d p_0_com_O_;   // 此刻实际位置 // BUG

    Vector3d v_k_com_d_O_; // 期望质心速度(2.21)
    Vector3d p_k_com_d_O_; // 期望质心位置(2.63)
    Vector3d omega_k_d_O_; // 期望角速度(2.65)
    Vector3d THETA_k_d_;   // 期望欧拉角(2.76)

    Matrix<double, 13 * PREDICTION_HORIZON, 1> D_;
    // slope estimate
    Matrix<double, 4, 3> p_stend_O_; // 4条腿的最近一次支撑相足底坐标
    Vector3d A_pla_hat_;                // 估计的地面平面系数

public:
    TrajectoryGenerator();
    ~TrajectoryGenerator();

    // 设置四足最近一次支撑相足底坐标，来自步态规划器
    void setState(const Matrix<double, 4, 3> &p_stend_O);
    // 设置期望值，来自上位机或手柄
    void setDesired(const double &v_x, const double &v_y, const double &omega_z);
    // 计算期望轨迹向量:[THETA_k_d,p_k_com_d_O,v_k_com_d_O,omega_k_d_O]*PREDICTION_HORIZON，其中包括坡度估计
    void calculateAll(const Matrix3d &Rot);
    // 返回期望轨迹向量(13h*1)
    void getD(Matrix<double, 13 * PREDICTION_HORIZON, 1> &vectorForD);

    void getpsi_k_d(Matrix<double, PREDICTION_HORIZON, 1> &vectorForpsi_k_d);

    // Vector3d *getv_k_com_d_O(int k, Matrix3d *Rot);
    // Vector3d *getp_k_com_d_O(int k, Vector3d *p_0_com_O, Matrix3d *Rot);
    // Vector3d *getomega_k_d_O();
    // Vector3d *getTHETA_k_d();
};

#endif